package Estados;

import teams.ucmTeam.RobotAPI;
import EDU.gatech.cc.is.util.Vec2;
import MaquinaEstados.Estado;
import MaquinaEstados.MaquinaEstados;

public class EstadoPortero extends Estado{
	
	public EstadoPortero(MaquinaEstados miMaquina) {
		super(miMaquina);
	}

	@Override
	protected void onEnd(RobotAPI robot) {
		robot.setDisplayString("");	
	}

	@Override
	protected void onInit(RobotAPI robot) {
		robot.setDisplayString("Portero");	
	}

	@Override
	protected void onTakeStep(RobotAPI robot) {

		
		robot.avoidCollisions();
		Vec2 posPorteria = robot.getOurGoal();
		Vec2 posBalon = robot.getBall();
		double y = 0;
		if (posBalon.y > 0.1)
			y = 0.1;
		else if (posBalon.y < -0.1)
			y = -0.1;
		else 
			y = posBalon.y;
			
		if (posBalon.r<0.3) {
		robot.setBehindBall(robot.getOpponentsGoal());
		if(robot.canKick())
			robot.kick(); 
		} 
		Vec2 destino = new Vec2(posPorteria.x, y);
		double t=destino.t;
		robot.setSteerHeading(t);
		robot.setSpeed(100.0);
		Vec2 dist = destino;
		dist.sub(robot.getPosition());
		if (dist.r<0.2){
			robot.setSpeed(0.0);
			robot.setSteerHeading(robot.getBall().t);
		}

	}
}
